
#include <cstdio>
#include <BMesh>
#include "member_BBody.h"
#include "member_BNode.h"
#include "member_BShape.h"

using namespace BWE;

member_BBody::member_BBody(BBody* body)
{
	boss = body;
	state = BBody::State_Active;
	continuous = false;
	kinematic = false;

	mass = 0;
	invMass = 0;
	gravity.set(0, 0, -10);

	restitution = BReal(0.1);
	friction = BReal(0.5);
	rollingFriction = BReal(0.1);

	deactivatedTime = 2;
	deactivatingTime = 0;
	linearThreshold = BReal(0.01);
	angularThreshold = BReal(0.01);

	dirty = true;
}
member_BBody::~member_BBody()
{

}

void member_BBody::updateInertia()
{
	if (node_member(boss)->shapes.empty() || mass == 0)
	{
		invMass = 0;
		inertia.reset();
		invInertia.reset();
		return;
	}
	invMass = BReal(1) / mass;
	if (node_member(boss)->shapes.size() == 1)
	{
		BShape* shape = node_member(boss)->shapes.first();
		if (shape_member(shape)->proxy)
			shape = shape_member(shape)->proxy;
		inertia = shape_member(shape)->calculateInertia(mass);
		invInertia.x() = inertia.x() > REAL_EPSILON ? 1 / inertia.x() : 0;
		invInertia.y() = inertia.y() > REAL_EPSILON ? 1 / inertia.y() : 0;
		invInertia.z() = inertia.z() > REAL_EPSILON ? 1 / inertia.z() : 0;
	}
	else
	{
		const BBox& box = boss->box();

		BReal lx = 2 * box.lena();
		BReal ly = 2 * box.lenb();
		BReal lz = 2 * box.lenc();

		inertia.x() = mass / 12 * (ly * ly + lz * lz);
		inertia.y() = mass / 12 * (lx * lx + lz * lz);
		inertia.z() = mass / 12 * (lx * lx + ly * ly);

		invInertia.x() = inertia.x() > REAL_EPSILON ? 1 / inertia.x() : 0;
		invInertia.y() = inertia.y() > REAL_EPSILON ? 1 / inertia.y() : 0;
		invInertia.z() = inertia.z() > REAL_EPSILON ? 1 / inertia.z() : 0;
	}
}
void member_BBody::updateSolverData(BReal timeStep)
{
	if (mass > REAL_EPSILON)
	{
		solverData.body = boss;
		solverData.forceVelocity = (gravity * mass + force) * invMass * timeStep;
		solverData.torqueVelocity = (torque) * invInertia * timeStep;
	}
	else
	{
		solverData.body = 0;
		solverData.forceVelocity.set(0);
		solverData.torqueVelocity.set(0);
	}
	solverData.pushVelocity.set(0);
	solverData.turnVelocity.set(0);
	solverData.linearVelocity.set(0);
	solverData.angularVelocity.set(0);
	solverData.hitFraction = 1;
}
void member_BBody::updateDeactivating(BReal timeStep)
{
	BReal linearThreshold2 = linearThreshold * linearThreshold;
	BReal angularThreshold2 = angularThreshold * angularThreshold;
	BReal linearLen2 = linearVelocity.length2();
	BReal angularLen2 = angularVelocity.length2();
	if (linearLen2 < linearThreshold2 && angularLen2 < angularThreshold2)
	{
		if (state == BBody::State_Active)
		{
			deactivatingTime += timeStep;
			boss->setState(BBody::State_Deactivating);
			return;
		}
		if (state == BBody::State_Deactivating)
		{
			deactivatingTime += timeStep;
			if (deactivatingTime > deactivatedTime)
			{
				boss->setState(BBody::State_Sleeping);
				deactivatingTime = 0;
				linearVelocity.reset();
				angularVelocity.reset();
			}
			return;
		}
	}
	else
	{
		if (state != BBody::State_Active)
			boss->setState(BBody::State_Active);
	}
}
BVector member_BBody::angularComponent(const BVector& vector)
{
	const BMatrix& matrix = node_member(boss)->matrix;
	BVector angular = matrix.inverse().mult3(vector) * invInertia;
	return matrix.mult3(angular);
}
